///////////////////////////////////////////////////////////////////////////////
//
// lighthouse3D - 3D Software Library
//
// File: CMathe3D.cpp
//
///////////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2003-2013 Marcus Reinhardt
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
///////////////////////////////////////////////////////////////////////////////
//
// You can find a copy of the GNU General Public License (gpl.txt) in the
// directory of the library.
//
///////////////////////////////////////////////////////////////////////////////
#include "CMathe3D.h"

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::CMathe3D( ... )
///////////////////////////////////////////////////////////////////////////////
CMathe3D::CMathe3D()
{
  float fl=-1;
  
  for (int i=0; i<360; i++) {
    fl = fl + 1;
    sinC[i] = (float) sin(fl / 180 * M_PI);
    cosC[i] = (float) cos(fl / 180 * M_PI);
  }
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::~CMathe3D( ... )
///////////////////////////////////////////////////////////////////////////////
CMathe3D::~CMathe3D()
{
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::round( ... )
///////////////////////////////////////////////////////////////////////////////
int CMathe3D::round(float x)
{
  return ((int) (x+0.5));
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::powerof( ... )
///////////////////////////////////////////////////////////////////////////////
// powerof(x,y) = x^y
///////////////////////////////////////////////////////////////////////////////
float CMathe3D::powerof(float x, int y)
{
  float e;
  int i;

  if ( y == 1 )
      return x;
      
  e = x;
  for (i=1; i<y; i++)
       e *= x;
       
  return e;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::sqr( ... )
///////////////////////////////////////////////////////////////////////////////
float CMathe3D::sqr(float x)
{
  return (x*x);
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::fastinvsqrt( ... )
///////////////////////////////////////////////////////////////////////////////
float CMathe3D::fastinvsqrt(float f)
{
	float half = 0.5f * f;
	int i = *(int*)&f;
	i = 0x5f3759df - (i >> 1);
	f = *(float*)&i;
	f = f * (1.5f - half * f * f);
	return f;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::fastinv( ... )
///////////////////////////////////////////////////////////////////////////////
float CMathe3D::fastinv(float x)
{
	// A little to unprices for good looking textures...
	/*float r;

	int _i = 2 * 0x3F800000 - *(int *)&(x);
	(r) = *(float *)&_i;
	(r) = (r) * (2.0f - (x) * (r));*/

	return 1/x;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::vectorLength( ... )
///////////////////////////////////////////////////////////////////////////////
float CMathe3D::vectorLength(TVector3D v)
{
  return (sqrt(sqr(v.d[0])+sqr(v.d[1])+sqr(v.d[2])));
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::CMathe3D::dotProduct( ... )
///////////////////////////////////////////////////////////////////////////////
float CMathe3D::dotProduct(TVector3D v1, TVector3D v2)
{
  return (v1.d[0]*v2.d[0]+v1.d[1]*v2.d[1]+v1.d[2]*v2.d[2]);
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::normalizeVector( ... )
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::normalizeVector(TVector3D v)
{
  float dist, invdist;

  /*dist = fastinvsqrt(sqr(v.d[0])+sqr(v.d[1])+sqr(v.d[2]));
  v.d[0] *= dist;
  v.d[1] *= dist;
  v.d[2] *= dist;

  return v;*/

  // Old routine
  dist = vectorLength(v);

  if ( dist != 0 ) {
	invdist=fastinv(dist);
	v.d[0] *= invdist;
	v.d[1] *= invdist;
	v.d[2] *= invdist;

	return v;
  }
  else
  {
	// not that good...
	v.d[0] = 1;
	v.d[1] = 0;
	v.d[2] = 0;

	return v;
  }
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::multSkVector( ... )
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::multSkVector(float sk, TVector3D v)
{
  v.d[0] = sk * v.d[0];
  v.d[1] = sk * v.d[1];
  v.d[2] = sk * v.d[2];

  return v;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::CMathe3D::addVectors( ... )
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::addVectors(TVector3D v1, TVector3D v2)
{
  TVector3D v;

  v.d[0] = v1.d[0] + v2.d[0];
  v.d[1] = v1.d[1] + v2.d[1];
  v.d[2] = v1.d[2] + v2.d[2];

  return v;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::rotation( ... )
///////////////////////////////////////////////////////////////////////////////
//
// performs a rotation
//
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::rotateVector(TVector3D v,  int ax, int ay, int az)
{
  v = transform(v, genRotMatrix(1, ax));
  v = transform(v, genRotMatrix(2, ay));
  v = transform(v, genRotMatrix(3, az));

  return v;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::CMathe3D::subVectors( ... )
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::subVectors(TVector3D v1, TVector3D v2)
{
  TVector3D v;

  v.d[0] = v1.d[0] - v2.d[0];
  v.d[1] = v1.d[1] - v2.d[1];
  v.d[2] = v1.d[2] - v2.d[2];

  return v;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::doV3D( ... )
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::doV3D(float x, float y, float z)
{
  TVector3D v;
  v.d[0] = x;
  v.d[1] = y;
  v.d[2] = z;

  return v;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::crossProduct( ... )
//
// Calculates the normal vector of two vectors
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::crossProduct(TVector3D v1, TVector3D v2)
{
   return doV3D(v1.d[1]*v2.d[2]-v1.d[2]*v2.d[1],
              v1.d[2]*v2.d[0]-v1.d[0]*v2.d[2],
              v1.d[0]*v2.d[1]-v1.d[1]*v2.d[0]);
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::matrixMul( ... )
///////////////////////////////////////////////////////////////////////////////
TMatrix4x4 CMathe3D::matrixMul(TMatrix4x4 m1, TMatrix4x4 m2)
{
  int i,j;
  float ab;
  TMatrix4x4 r;

  for(i=0; i<4; i++)
  {
	  for(j=0; j<4; j++)
	  {
		  ab=0;
		  // for(k=0; k<4; k++)
		  //	ab += m1.d[i][k]*m2.d[k][j];
		  ab += m1.d[i][0]*m2.d[0][j];
		  ab += m1.d[i][1]*m2.d[1][j];
		  ab += m1.d[i][2]*m2.d[2][j];
		  ab += m1.d[i][3]*m2.d[3][j];

		  r.d[i][j] = ab;
	  }
  }

  return r;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::genRotMatrix( ... )
///////////////////////////////////////////////////////////////////////////////
//
// generates a rotation matrix
//
///////////////////////////////////////////////////////////////////////////////
TMatrix4x4 CMathe3D::genRotMatrixComplex(TMatrix4x4 A, int m, int theta)
{
  int m1,m2;
  float c,s;

  A.d[m-1][m-1] = 1;
  A.d[3][3] = 1;
  
  m1 = (m%3)+1;
  m2 = (m1%3);
  m1 -= 1;
  
  c = cosC[theta];
  s = sinC[theta];
  
  A.d[m1][m1] = c;
  A.d[m1][m2] = s;
  A.d[m2][m2] = c;
  A.d[m2][m1] = -s;

  return A;
}

TMatrix4x4 CMathe3D::genRotMatrix(int m, int theta)
{
  TMatrix4x4 A;
  int m1,m2;
  float c,s;

  A = genZeroMatrix();

  A.d[m-1][m-1] = 1;
  A.d[3][3] = 1;
  
  m1 = (m%3)+1;
  m2 = (m1%3);
  m1 -= 1;
  
  c = cosC[theta];
  s = sinC[theta];
  
  A.d[m1][m1] = c;
  A.d[m1][m2] = s;
  A.d[m2][m2] = c;
  A.d[m2][m1] = -s;

  return A;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::genSkalMatrix( ... )
///////////////////////////////////////////////////////////////////////////////
//
// generates a skale matrix
//
///////////////////////////////////////////////////////////////////////////////
TMatrix4x4 CMathe3D::genSkalMatrix(float sx, float sy, float sz)
{
  TMatrix4x4 A;
  A = genZeroMatrix();

  A.d[0][0] = sx;
  A.d[1][1] = sy;
  A.d[2][2] = sz;
  A.d[3][3] = 1;

  return A;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::genTranMatrix( ... )
///////////////////////////////////////////////////////////////////////////////
//
// generates a translation matrix
//
///////////////////////////////////////////////////////////////////////////////
TMatrix4x4 CMathe3D::genTranMatrix(float tx, float ty, float tz)
{
  TMatrix4x4 A;
  A = genZeroMatrix();

  A.d[0][0] = 1;
  A.d[1][1] = 1;
  A.d[2][2] = 1;
  A.d[3][3] = 1;

  A.d[0][3] = tx;
  A.d[1][3] = ty;
  A.d[2][3] = tz;

  return A;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::genZeroMatrix( ... )
///////////////////////////////////////////////////////////////////////////////
//
// generates a zero matrix
//
///////////////////////////////////////////////////////////////////////////////
TMatrix4x4 CMathe3D::genZeroMatrix()
{
  TMatrix4x4 r;

  for(int i=0; i<4; i++) {
    for(int k=0; k<4; k++) {
      r.d[i][k] = 0;
    }
  }

  return r;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::transform( ... )
///////////////////////////////////////////////////////////////////////////////
//
// perform a transformation
//
///////////////////////////////////////////////////////////////////////////////
TVector3D CMathe3D::transform(TVector3D v, TMatrix4x4 m)
{
  TVector3D r;

  r.d[0] = m.d[0][0] * v.d[0] + m.d[0][1] * v.d[1] +
           m.d[0][2] * v.d[2] + m.d[0][3];
  r.d[1] = m.d[1][0] * v.d[0] + m.d[1][1] * v.d[1] +
           m.d[1][2] * v.d[2] + m.d[1][3];
  r.d[2] = m.d[2][0] * v.d[0] + m.d[2][1] * v.d[1] +
           m.d[2][2] * v.d[2] + m.d[2][3];

  return r;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::projection( ... )
///////////////////////////////////////////////////////////////////////////////
//
// performs a projection
//
///////////////////////////////////////////////////////////////////////////////
bool CMathe3D::projection(TMeshVertex *v, float pjDist, int scX, int scY)
{
  float n;
  float invn;

  if (v == NULL)
	  return false;

  n = v->zt;
  invn = fastinv(n);

  // if (!n)
  //	  n = 1;

  v->sx = (int) (pjDist * v->xt * invn) + scX;
  v->sy = (int) (pjDist * (-1) * v->yt * invn) + scY;
  // v->sx = (int) (pjDist * v->xt / n) + scX;
  // v->sy = (int) (pjDist * (-1) * v->yt / n) + scY;

  return true;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::projection( ... )
///////////////////////////////////////////////////////////////////////////////
//
// performs a projection using the camera data
//
///////////////////////////////////////////////////////////////////////////////
bool CMathe3D::projectionCamera(TMeshVertex *v, float pjDist, int scX, int scY)
{
  float n;
  float invn;

  if (v == NULL)
	  return false;

  n = v->ztc;
  invn = fastinv(n);

  //if (!n)
  //	  n = 1;

  v->sx = (int) (pjDist * v->xtc * invn) + scX;
  v->sy = (int) (pjDist * (-1) * v->ytc * invn) + scY;
  // v->sx = (int) (pjDist * v->xtc / n) + scX;
  // v->sy = (int) (pjDist * (-1) * v->ytc / n) + scY;

  return true;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::rotation( ... )
///////////////////////////////////////////////////////////////////////////////
//
// performs a rotation
//
///////////////////////////////////////////////////////////////////////////////
bool CMathe3D::rotation(TMeshVertex *v,  int ax, int ay, int az)
{
  TVector3D v3D;

  v3D.d[0] = v->xt;
  v3D.d[1] = v->yt;
  v3D.d[2] = v->zt;

  /*
   * v3D = transform(v3D, genRotMatrix(1, ax) );
   * v3D = transform(v3D, genRotMatrix(2, ay) );
   * v3D = transform(v3D, genRotMatrix(3, az) );
   */

  v3D = transform(v3D, matrixMul( genRotMatrix(3, az),
					   matrixMul( genRotMatrix(2, ay),
					              genRotMatrix(1, ax) ) ) );

  v->xt = v3D.d[0];
  v->yt = v3D.d[1];
  v->zt = v3D.d[2];

  return true;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::skalation( ... )
///////////////////////////////////////////////////////////////////////////////
//
// performs a skalation
//
///////////////////////////////////////////////////////////////////////////////
bool CMathe3D::skalation(TMeshVertex *v,  float sx, float sy, float sz)
{
  TVector3D v3D;

  v3D.d[0] = v->xt;
  v3D.d[1] = v->yt;
  v3D.d[2] = v->zt;

  v3D = transform(v3D, genSkalMatrix(sx, sy, sz));

  v->xt = v3D.d[0];
  v->yt = v3D.d[1];
  v->zt = v3D.d[2];

  return true;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::translation( ... )
///////////////////////////////////////////////////////////////////////////////
//
// performs a translation
//
///////////////////////////////////////////////////////////////////////////////
bool CMathe3D::translation(TMeshVertex *v,  float tx, float ty, float tz)
{
  TVector3D v3D;

  v3D.d[0] = v->xt;
  v3D.d[1] = v->yt;
  v3D.d[2] = v->zt;

  v3D = transform(v3D, genTranMatrix(tx, ty, tz));

  v->xt = v3D.d[0];
  v->yt = v3D.d[1];
  v->zt = v3D.d[2];

  return true;
}

///////////////////////////////////////////////////////////////////////////////
// CMathe3D::translationCamera( ... )
///////////////////////////////////////////////////////////////////////////////
//
// performs a translation and stores the values in the camera fields
//
///////////////////////////////////////////////////////////////////////////////
bool CMathe3D::translationCamera(TMeshVertex *v,  float tx, float ty, float tz)
{
  TVector3D v3D;

  v3D.d[0] = v->xt;
  v3D.d[1] = v->yt;
  v3D.d[2] = v->zt;

  v3D = transform(v3D, genTranMatrix(tx, ty, tz));

  v->xtc = v3D.d[0];
  v->ytc = v3D.d[1];
  v->ztc = v3D.d[2];

  return true;
}

